#!/usr/bin/python
import rospy
from nav_msgs.msg import Odometry,Path
from geometry_msgs.msg import PoseStamped


path_pub = rospy.Publisher('robot_trajectory',Path,queue_size=10)
trajectory = Path()

def get_odom(data):
    global trajectory
    global path_pub

    this_pose_stamped = PoseStamped()
    this_pose_stamped.pose.position = data.pose.pose.position
    this_pose_stamped.pose.orientation = data.pose.pose.orientation
    this_pose_stamped.header.frame_id = data.header.frame_id
    this_pose_stamped.header.stamp = data.header.stamp
    trajectory.header.frame_id = this_pose_stamped.header.frame_id
    trajectory.header.stamp = this_pose_stamped.header.stamp
    trajectory.poses.append(this_pose_stamped)
    path_pub.publish(trajectory)

def listener():
    rospy.init_node("show_trajectory", anonymous=False)
    odom_sub = rospy.Subscriber('odom',Odometry,get_odom)
    rospy.loginfo('Start Show Robot trajectory')
    rospy.spin()

if __name__ == '__main__':
    try:
        listener()
    except:
        rospy.loginfo("trajectory terminated.")
